#include "ul_fifo_task_queue.h"
#include <errno.h>
#include <sched.h>
#include "ul_define.h"
#include "ul_malloc.h"
#include "ul_util.h"
#include "ul_atomic.h"
LinuxNetworkUtility::UlFIFO::UlFIFO()
{
  msg_queue_ = NULL;
  queue_size_ = 0;

  push_count_ = 0;
  push_idx_ = 0;
  pop_count_ = 0;
  pop_idx_ = 0;

  consumer_num_ = 0;
  consumers_  = NULL;

  has_waiting_consumer_ = 0;
}

LinuxNetworkUtility::UlFIFO::~UlFIFO()
{
  destroy(); 
}


void LinuxNetworkUtility::UlFIFO::destroy()
{
  ul_free(msg_queue_);
  queue_size_ = 0;

  push_count_ = 0;
  push_idx_ = 0;

  consumer_num_ = 0;
  ul_free(consumers_);

  has_waiting_consumer_ = 0;

  pthread_mutex_destroy(&cond_mutex_);
  pthread_cond_destroy(&cond_);
}

int LinuxNetworkUtility::UlFIFO::init(const int64_t queue_size, const int64_t consumer_num)
{
  int err = 0;
  if (queue_size <= 0 || consumer_num <= 0)
  {
    UL_LOG_WARNING("param error [queue_size:%ld,consumer_num:%ld]", queue_size, consumer_num);
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    void *buf = NULL;
    buf = ul_malloc(sizeof(FIFOMsgHead)*queue_size, UlModIds::UL_MOD_DEFAULT, NULL, FREE_TO_GLOBAL_POOL);
    if (NULL == buf)
    {
      UL_LOG_WARNING("fail to allocate memory for queue");
      err = ERRNO2ERR(ENOMEM);
    }
    else
    {
      msg_queue_ = new(buf)FIFOMsgHead[queue_size];
    }
    if (0 == err)
    {
      buf = ul_malloc(sizeof(FIFOConsumerInfo)*consumer_num, UlModIds::UL_MOD_DEFAULT,NULL, FREE_TO_GLOBAL_POOL);
      if (NULL == buf)
      {
        UL_LOG_WARNING("fail to allocate memory for consumer infos");
        err = ERRNO2ERR(ENOMEM);
      }
      else
      {
        consumers_ = new(buf)FIFOConsumerInfo[consumer_num];
      }
    }
  }
  if (0 == err)
  {
    queue_size_ = queue_size;
    max_idx_ = queue_size_ - 1;
    consumer_num_ = consumer_num;
    pthread_mutex_init(&cond_mutex_, NULL);
    pthread_cond_init(&cond_,NULL);
  }
  if (0 != err)
  {
    destroy();
  }
  errno = ERRNO2ERR(err);;
  return err;
}

void *LinuxNetworkUtility::UlFIFO::pop(const int64_t consumer_id,const FetchFlag flag)
{
  void *result = NULL;
  int err = 0;
  if (consumer_id < 0 || consumer_id >= consumer_num_)
  {
    UL_LOG_WARNING("argument error [consumer_id:%ld,consumer_num_:%ld]", consumer_id, consumer_num_);
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err && NULL == msg_queue_)
  {
    UL_LOG_WARNING("fifo was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if ((0 == err) && (flag & FETCH_EVERY_MSG) && (flag & FETCH_UNFETCHED_MSG))
  {
    UL_LOG_WARNING("param error [flag:%d]",flag);
    err  = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    do
    {
      if (consumers_[consumer_id].pop_count_ != push_count_)
      {
        int32_t unfetched_consumer_num = 0;
        result = msg_queue_[consumers_[consumer_id].pop_idx_].fetch_msg(flag, unfetched_consumer_num);
        if (0 == unfetched_consumer_num)
        {
          pop_count_ ++;
          if (max_idx_ == pop_idx_)
          {
            pop_idx_ = 0;
          }
          else
          {
            pop_idx_ ++;
          }
        }
        consumers_[consumer_id].pop_count_  ++;
        if (max_idx_ == consumers_[consumer_id].pop_idx_ )
        {
          consumers_[consumer_id].pop_idx_ = 0;
        }
        else
        {
          consumers_[consumer_id].pop_idx_ ++;
        } 
      }
      else
      {
        if (flag & FETCH_BLOCKING)
        {
          struct timespec abstime;
          ul_make_abstime(abstime,1000);
          has_waiting_consumer_ = 1;
          pthread_mutex_lock(&cond_mutex_);
          /// wait maximum 1ms
          pthread_cond_timedwait(&cond_, &cond_mutex_, &abstime);
          pthread_mutex_unlock(&cond_mutex_);
        }
      }
    }while (NULL == result && (flag & FETCH_BLOCKING));
  }
  errno = ERRNO2ERR(err);
  return result;
}


int LinuxNetworkUtility::UlFIFO::push(void *msg)
{
  int err = 0;
  if (NULL == msg_queue_)
  {
    UL_LOG_WARNING("fifo was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    if (push_count_ - pop_count_ < static_cast<uint64_t>(queue_size_))
    {
      msg_queue_[push_idx_].init(consumer_num_,msg);
      if (push_idx_ == max_idx_)
      {
        push_idx_ = 0;
      }
      else
      {
        push_idx_ ++;
      }
      push_count_ ++;
      if (has_waiting_consumer_)
      {
        has_waiting_consumer_ = 0;
        pthread_cond_broadcast(&cond_);
      }
    }
    else
    {
      err = ERRNO2ERR(EAGAIN);
    }
  }
  errno = ERRNO2ERR(err);
  return err;
}

void *LinuxNetworkUtility::UlFIFO::FIFOMsgHead::fetch_msg(const FetchFlag flag, 
                                                          int32_t & unfetched_consumer_num)
{
  void *result = NULL;
  const int32_t minus_one = -1;
  int32_t old_val = 0;
  if (fetched_ == 0 && atomic_add((uint32_t*)&fetched_,1) == 0)
  {
    result = msg_;
  }
  else
  {
    if (flag & FETCH_EVERY_MSG)
    {
      result = msg_;
    }
  }
  old_val = atomic_add((uint32_t*)&unfected_consumer_num_,(uint32_t)minus_one);
  if (old_val <= 0)
  {
    UL_LOG_ERROR("algorithm error, unfetched consumer number is less than 0 [old_val:%d]", 
                 old_val);
  }
  unfetched_consumer_num = old_val - 1;
  return result;
}

LinuxNetworkUtility::UlFIFOQueenWorkerTQ::UlFIFOQueenWorkerTQ()
{
  worker_num_ = 0;
  worker_end_fifos_ = NULL;
  prev_fetch_worker_idx_  = 0;
}


LinuxNetworkUtility::UlFIFOQueenWorkerTQ::~UlFIFOQueenWorkerTQ()
{
  prev_fetch_worker_idx_  = 0;
  destroy();
}

void LinuxNetworkUtility::UlFIFOQueenWorkerTQ::destroy()
{
  if (NULL != worker_end_fifos_)
  {
    for (int64_t i = 0; i < worker_num_; i++)
    {
      worker_end_fifos_[i].~UlFIFO();
    }
  }
  worker_num_ = 0;
  ul_free(worker_end_fifos_);
}

int LinuxNetworkUtility::UlFIFOQueenWorkerTQ::init(const int64_t worker_bee_num, const int64_t queue_size)
{
  int err = 0;
  void *buf = NULL;
  if (worker_bee_num <= 0)
  {
    UL_LOG_WARNING("param error [worker_bee_num:%ld]", worker_bee_num);
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    buf = ul_malloc(sizeof(UlFIFO)*worker_bee_num, UlModIds::UL_MOD_DEFAULT,NULL,FREE_TO_GLOBAL_POOL);
    if (NULL == buf)
    {
      UL_LOG_WARNING("fail to allocate memory for worker end fifos");
      err = ERRNO2ERR(ENOMEM);
    }
    else
    {
      worker_end_fifos_ = new(buf)UlFIFO[worker_bee_num];
      worker_num_ = worker_bee_num;
    }
  }
  if (0 == err)
  {
    err = queen_end_fifo_.init(queue_size,worker_bee_num);
  }
  if (0 == err)
  {
    for (int64_t i = 0; i < worker_num_ && 0 == err; i++)
    {
      err = worker_end_fifos_[i].init(queue_size,1);
    }
  }
  if (0 != err)
  {
    destroy();
  }
  errno = ERRNO2ERR(err);
  return err;
}

int LinuxNetworkUtility::UlFIFOQueenWorkerTQ::dispatch_task(void *tsk)
{
  int err = 0;
  if (NULL == worker_end_fifos_)
  {
    UL_LOG_WARNING("task queue was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    err = queen_end_fifo_.push(tsk);
  }
  errno = ERRNO2ERR(err);
  return err;
}

int LinuxNetworkUtility::UlFIFOQueenWorkerTQ::claim_task(void **tsk, const int64_t ntsk, 
                                                         const int64_t worker_id, 
                                                         const UlFIFO::FetchFlag flag)
{
  int err = 0;
  int got_tsk_num = 0;
  if (NULL == worker_end_fifos_)
  {
    UL_LOG_WARNING("task queue was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    if (NULL == tsk || 0 >= ntsk || worker_id < 0 || worker_id >= worker_num_)
    {
      err = ERRNO2ERR(EINVAL);
      UL_LOG_WARNING("[tsk:%p,ntsk:%ld,worker_id:%ld,worker_num:%ld]", tsk, ntsk, worker_id, worker_num_);
    }
  }
  if (0 == err)
  {
    UlFIFO::FetchFlag real_flag =static_cast<UlFIFO::FetchFlag>((flag | UlFIFO::FETCH_BLOCKING));
    for (; got_tsk_num < ntsk; got_tsk_num++)
    {
      tsk[got_tsk_num] = queen_end_fifo_.pop(worker_id,real_flag);
      if (NULL == tsk[got_tsk_num])
      {
        err = ERRNO2ERR(errno);
        break;
      }
    }
  }
  errno = ERRNO2ERR(err);
  if (got_tsk_num > 0)
  {
    err = got_tsk_num;
  }
  return err;
}


int LinuxNetworkUtility::UlFIFOQueenWorkerTQ::submit_output(void *tsk, const int64_t worker_id)
{
  int err = 0;
  if (NULL == worker_end_fifos_)
  {
    UL_LOG_WARNING("task queue was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if ( 0 == err && worker_id >= worker_num_)
  {
    UL_LOG_WARNING("worker_id:%ld,worker_num:%ld]", worker_id, worker_num_);
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    err = worker_end_fifos_[worker_id].push(tsk);
  }
  errno = ERRNO2ERR(err);
  return err;
}


int LinuxNetworkUtility::UlFIFOQueenWorkerTQ::fetch_output(void **tsk, const int64_t ntsk)
{
  int err = 0;
  int32_t got_tsk_num = 0;
  if (NULL == worker_end_fifos_)
  {
    UL_LOG_WARNING("task queue was not initialized yet");
    err = ERRNO2ERR(EINVAL);
  }
  if (0 == err)
  {
    if (NULL == tsk || ntsk <= 0)
    {
      UL_LOG_WARNING("param error [tsk:%p,ntsk:%ld]", tsk, ntsk);
      err = ERRNO2ERR(EINVAL);
    }
  }

  if (0 == err)
  {
    UlFIFO::FetchFlag flag = UlFIFO::FETCH_EVERY_MSG;
    int64_t cur_worker = prev_fetch_worker_idx_;
    int64_t fetched_worker_num = 0;
    for (;got_tsk_num < ntsk && fetched_worker_num < worker_num_; )
    {
      while (NULL == tsk[got_tsk_num] && fetched_worker_num < worker_num_)
      {
        tsk[got_tsk_num] = worker_end_fifos_[cur_worker].pop(0,flag);
        if (NULL == tsk[got_tsk_num])
        {
          cur_worker = (cur_worker + 1)%worker_num_;
          fetched_worker_num ++;
        }
      } 
      if (NULL != tsk[got_tsk_num])
      {
        got_tsk_num ++;
      }
    }
  }
  if (got_tsk_num > 0)
  {
    err = got_tsk_num;
  }
  return err;
}
